import java.io.File;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.TreeMap;

public class Assignment4 {

	static final int            DISTINGUISH_MAP_VALUE     = 155;
	static final int            ROBOT_COLOR               = 100;
	private static final int    PIXEL_TO_CM               = 4;
	private static final double MAXIMUM_MEASUREMENT_RANGE = 15 * ( (double)100 / (double)(PIXEL_TO_CM) );
	private static final double SIGMA_ROBOT               = 15.0;
	private static final int ANGLE_STEP = 2;
	private static final int ARRAY_OFFSET = 800*10;
	private static final int ROBOT_SIZE = 4;
	private static float[][] distanceCache = new float[400+ARRAY_OFFSET][680+ARRAY_OFFSET];
	
	
	static HashMap<Cordinate, Float> minimalDistances = new HashMap<Cordinate, Float>();

	public static void main(String[] args) {
		
		File file=new File("Assignment_4_Grid_Map.png"); //file of the image
		ImageReader image=new ImageReader(file); // Initialize reader
		
		int[][] map;
		map = image.getImageToArray();	 // read image into array
		
		//robot position, we put him in the center of the map
		int robotXcord = map.length/2; // robot position x
		int robotYcord = map[0].length/2; // robot position y
		int robotAngle = 0; //robot direction angle is 0 (theta is measured from x axis)
			
		int openingAngle = 270; // from exercise given (can be changed)
		int angleStep    = ANGLE_STEP; //from exercise given (can be changed)
		int[][] measurments;
		measurments = performRayCasting(robotXcord, robotYcord, robotAngle, openingAngle, angleStep, map);
		drawCircle(ROBOT_SIZE, robotXcord, robotYcord, map); //robot position mark in the picture as green
		
		//draw measured points
		for (int i=0; i<measurments.length; i++){
			map[ measurments[i][1] ] [ measurments[i][2] ] = DISTINGUISH_MAP_VALUE;
		}
		
		image.saveImage(map, "ex1_part1.png");
		
		
		int[][] mapSecondTask = image.getImageToArray();
		int[] measurment = oneBeamGeneration(robotXcord, robotYcord, robotAngle , 0 , mapSecondTask);
		
		
		double[][][]likelihoodProbality = new double[map.length][map[0].length][360];
		int[][] grayScaleValues = new int[map.length][map[0].length];
		
		int counter = 0;
		for ( int i = 0; i < mapSecondTask.length ; i++ )
		{
			for (int j = 0; j< mapSecondTask[i].length; j++ )
			{
				if ( mapSecondTask[i][j] == 0 )
				{
					counter++;
				}
			}
		}
		
		BlockedCord [] blocked = new BlockedCord[counter];
		counter = 0;
		for ( int i = 0; i < mapSecondTask.length ; i++ )
		{
			for (int j = 0; j< mapSecondTask[i].length; j++ )
			{
				if ( mapSecondTask[i][j] == 0 )
				{
					blocked[counter++] = new BlockedCord(i,j);
				}
			}
		}		
		
		double probArray [][] = new double [map.length][map[0].length];
		
		double maxProb = 0;
		for ( int x = 0; x < map.length; x++ )
		{
			for ( int y = 0; y < map[0].length; y++ )
			{
				double accumProb = 0;
				for ( int theta = 0; theta < 360; theta++ )
				{
					likelihoodProbality[x][y][theta] = likelihoodFieldRangeFinderModel(x, y, theta, measurments, robotXcord, robotYcord, blocked);
					accumProb += likelihoodProbality[x][y][theta];
				}
				System.out.println("Prob for cordinate ["+x+","+y+"] = "+accumProb);

				probArray[x][y] = accumProb;
				
				if ( accumProb > maxProb )
					maxProb = accumProb;
			}
		}
		
		for ( int x = 0; x < map.length; x++ )
		{
			for ( int y = 0; y < map[0].length; y++ )
			{
				grayScaleValues[x][y] = (int)(255 - (int)(255.0 * (probArray[x][y]/maxProb)) );
			}
		}

		
		image.saveImage(map,"ex1_part2.png",grayScaleValues);

		
		
		// CALCULATING PROBABLITY FOR ONE BEAM
		/*
		int[][]arryaForProbability;
		arryaForProbability = image.getImageToArray();	 // read image into array
		int []probabilityMeasurment1; //MEASURMENT ONE FOR A SINGLE BEAM
		probabilityMeasurment1=oneBeamGeneration(robotXcord, robotYcord, robotAngle , 0 , arryaForProbability);
		drawCircle(4, robotXcord, robotYcord, arryaForProbability); //robot position mark in the picture as green
		
		int []probabilityMeasurment2; //MEASURMENT TWO FOR A SINGLE BEAM
		probabilityMeasurment2=oneBeamGeneration(robotXcord+10, robotYcord, robotAngle , 0 , arryaForProbability);
		drawCircle(4, robotXcord+10, robotYcord, arryaForProbability); //robot position mark in the picture as green
		
		
		image.saveImage(arryaForProbability, "ex1_part2.png");	*/
	}
	
	public static void drawCircle(int radius, int position_x, int position_y, int[][] array){
		for(int i = 0; i < 360; i++){
			int angle = i;
	        int x1 = (int) Math.round((radius * Math.cos(angle * Math.PI / 180)));
	        int y1 = (int)  Math.round((radius * Math.sin(angle * Math.PI / 180)));
	        array[position_x + x1][position_y + y1]=100;
	    }
	}
	
	/** universal function returns the measurement points and draws lines of rail cast**/
	public static int[][] performRayCasting(int robotX, int robotY, int theta , int openingAngle, int angleStep, int[][] map){
		
		int[][] measurments=new int[openingAngle/angleStep+1][3]; // array of measurements
																  //   first column is beam theta
		 														  // - second column is x coordinate of measurement point
		 														  // - third column is y coordinate of measurement point

		for ( int angle = (-openingAngle / 2 + theta ), raysCount = 0;
				angle < ( openingAngle / 2 + theta );
				angle += angleStep, raysCount++ )
		{ // for every angle of ray beam		
			measurments[raysCount] = oneBeamGeneration( robotX, robotY, theta, angle, map );
		}	
		return measurments;
	}
	
	public static int[] oneBeamGeneration(int robotX, int robotY, int robotTheta , int angle, int[][] map ){
		
		int[] measurments=new int[4]; // array of measurements
																  //   first column is beam theta
		 														  // - second column is x coordinate of measurement point
		 														  // - third column is y coordinate of measurement point		
		int xStep = 0;
		int yStep = 0;

		double radians = Math.toRadians(angle);
		
		while(true){ //do until obstacle is reached
			if ( (Math.abs(angle) < 45) || ( Math.abs(angle) > 145 && Math.abs(angle)<225 ) ){ // for every angle from -45 to 45
				if ( Math.abs(angle) < 45 ){
					xStep++; 
					yStep = (int)(Math.tan(radians)*xStep); // calculating y step and casting to integer so that we receive only discrete value set
				}
				else {
					xStep--;
					yStep = -(int)(Math.tan(radians)*xStep); // calculating y step and casting to integer so that we receive only discrete value set
				}
				
			}
			else if((Math.abs(angle)>=45 && Math.abs(angle)<=145) || (Math.abs(angle)>=225 && Math.abs(angle)<=315)){
				if ( angle>=45 && angle<=145 )
					yStep--;
				else if ( angle<=-45 && angle>=-145 )
					yStep++;
				else if (angle>=225 && angle<=315)
					yStep++;
				else
					yStep--;
					
				xStep=-(int)(Math.round((1.0/(Math.tan(radians)))*yStep)); //cotangenst
			}
			if ( Math.sqrt(xStep*xStep+yStep*yStep) >= MAXIMUM_MEASUREMENT_RANGE || // measurement greater than 15m or obstacle was reached
					map[robotX + xStep][ robotY+yStep ] == 0){ 
				measurments[0] = angle; //saving beam theta
				measurments[1] = robotX + xStep; //save x coordinate
				measurments[2] = robotY + yStep; //save y coordinate
				break; //obstacle reached => make next measurement for theta
			}
			
			map[ robotX + xStep ][robotY+yStep] = DISTINGUISH_MAP_VALUE;
		}
		return measurments;
	}
	public static double robotLikelihood( double distance, double sigma )
	{
		return ( 1.0/( sigma * Math.sqrt(2*Math.PI) ) ) *
				Math.exp( -(distance * distance)/(sigma*sigma*2.0));
	}
	
	public static double likelihoodFieldRangeFinderModel(int robotX, int robotY, int robotTheta, int[][] measurments, int robotXatMeasurmentTime, int robotYatMeasurmentTime, BlockedCord[] blocked )
	{
		
		double q = 1.0;
		for ( int k = 0; k < measurments.length; k++ )
		{
			float xOrig = measurments[k][1] - robotXatMeasurmentTime;
			float yOrig = measurments[k][2] - robotYatMeasurmentTime;
			float theta = measurments[k][0];
			
			float zK    = (float) Math.sqrt( Math.pow(xOrig, 2) + Math.pow(yOrig,2));
			
			if ( zK >= MAXIMUM_MEASUREMENT_RANGE )
				continue;
			
			float xZk   = (float) (robotX + ROBOT_SIZE*Math.cos(Math.toRadians(robotTheta)) -
					ROBOT_SIZE * Math.sin(Math.toRadians(robotTheta)) +
					zK*Math.cos(Math.toRadians(robotTheta + theta)));
			
			float yZk   = (float) (robotY + ROBOT_SIZE*Math.cos(Math.toRadians(robotTheta)) -
					ROBOT_SIZE * Math.sin(Math.toRadians(robotTheta)) +
					zK*Math.sin(Math.toRadians(robotTheta + theta)));
			
			float distance = Float.MAX_VALUE;

			if ( distanceCache[(int)(xZk) + ARRAY_OFFSET/2  ][(int)(yZk) + ARRAY_OFFSET/2] != 0 )
			{
				distance = distanceCache[(int)(xZk) +  ARRAY_OFFSET/2 ][(int)(yZk) + ARRAY_OFFSET/2];
			}
			
			else
			{
				for ( BlockedCord b: blocked )
				{
					float currentDistance =
					(float) (Math.pow((xZk - b.x),2) + 
					Math.pow((yZk - b.y),2));
					
					if ( currentDistance < distance )
					{
						distance = currentDistance;
					}
				}
				distance = (float) Math.sqrt(distance); // according to the book: square root should be taken, however in the Berkeley slides, no square root is taking
				distanceCache[(int)(xZk)+ARRAY_OFFSET/2][(int)(yZk)+ARRAY_OFFSET/2] = distance;
			}
			double prob = robotLikelihood( distance, SIGMA_ROBOT );
			q *= prob;
		}
		return q;
	}
}
